#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include "tf/transform_datatypes.h"
#include <iostream>
#include <string>
#include <sys/time.h>
#include <unistd.h>
#include <vector>
#include <mutex>
#include <condition_variable>
#include <time.h>

using namespace std;
enum routeMode {RECORD,HANDWRITE,MAKEUP,POINT};
enum taskState{TASK_OK,TASK_ERROR,FINISHED,CANCELED};
enum robotState{IDLE,RUNNING,INITIAL};
struct Route
{
    string name_;
    bool loop_;
    unsigned loop_count_;
    unsigned int points_num_;
    unsigned int current_point_;
    unsigned int current_loop_count_;
    vector<geometry_msgs::Pose> route_line_;
    routeMode mode_;
};
struct Task
{
    string start_time;
    string end_time;
    string task_name;
    string route_name;
    bool loop;
    int loop_count;
    bool finished;
};

class TaskManager
{
public:
    /**
     * @brief save all routes
     * @param 
     * @return return true if success     
     */
    bool setRouteLines(vector<string> route_line_names,\
         vector<vector<geometry_msgs::Pose> > route_lines);

    /**
     * @brief get next goal point
     * @return TASK_OK: get next point, NO_TASK: no more point 
     */
    taskState getGoal(move_base_msgs::MoveBaseGoal &goal);

    void setTaskList(vector<Task> task_list)
    {
        task_list_ = task_list;
        for(int i=0;i<task_list_.size();i++)
        {
            ;//ROS_INFO("start time:%s",task_list_[i].start_time.c_str());
        }
    }
    
    void setRobotPose(geometry_msgs::Pose pose)
    {
        robot_pose_ = pose;
    }

    void setPosState(bool state)
    {
        pos_valid_ = state;
    }
    bool isPosValid()
    {
        return pos_valid_;
    }
    /**
     * @brief dispatch task according time
     */
    void schedule();

    string name_;
    taskState task_state{FINISHED};

    vector<Task> task_; 
    
private:
    void getLoopTask(geometry_msgs::Pose &point);
    void getNoloopTask(geometry_msgs::Pose &point);

    bool arriveGoal();

    bool findNextTask(Task &task);
    void executeTask(Task task);

    bool cancelTask();

    /* data */
    Route current_route;
    Route new_route;
    bool got_new_route{false};

    /*routeMode mode_;
    bool loop_;
    unsigned loop_count_;
    unsigned int points_num_{0};
    unsigned int current_point_{0};
    unsigned int current_loop_count_{0};
    vector<geometry_msgs::Pose> route_line_;
*/
    vector<Task> task_list_;

    vector<string> route_line_names_;    
    vector<vector<geometry_msgs::Pose> > route_lines_;
    vector<bool> loops_;
    vector<unsigned int> loop_counts_;

    mutex mutex_; 
    bool task_valid_;

    robotState robot_state {IDLE};
    Task last_task;

    bool pos_valid_{false};
    geometry_msgs::Pose robot_pose_;
};


